/*
 * GeoPoint.java
 *
 * Created on 23 mai 2006, 09:14
 *
 */

package courselog.gpsutils;

import ewe.sys.Convert;
import ewe.sys.Time;
import ewe.util.Copyable;
import courselog.gpsutils.jcoord.RefEll;

/**
 * Base class for a location point on Earth.
 * @author C. Rigal
 */
public class GeoPoint implements Copyable {
    
    /**
     * Earth radius in meters.
     */
    public static double ER = 6366707.0d;
    /**
     * Dummy value to put coordinates to recognise that it was not initialise.
     * =1E10, Somewhere in space!
     */
    public static double dummyCoord=1E10d;
    /**
     * Dummy angle for initialisation.
     * Equals 1E10, it should not happen. Use this value to tell the
     * (lat,lon,alt) of the point is uninitialised.
     */
    public static double dummyAng=1E10d;
    
    /**
     * Altitude of the location to the Mean Sea Level (m).
     */
    public double altSeaLevel;
    
    /**
     * Latitude in degrees (positive North).
     */
    public double lat;
    
    /**
     * Longitude in degrees (Positive East).
     */
    public double lon;
    
    /**
     * X coordinate of this point (m).
     * @see GeoPoint#computeCartesian()
     */
    public double myX;
    /**
     * Y coordinate of this point (m).
     */
    public double myY;
    /**
     * Z coordinate of this point (m).
     */
    public double myZ;
    
    /**
     * UTM projection easting distance (m).
     * @see GeoPoint#computeUTMtoLatLng()
     */
    public double uTMEasting=0;
    /**
     * UTM projection Northing distance (m).
     */
    public double uTMNorthing=0;
    /**
     * UTM projection latitude zone (char).
     */
    public char uTMLatZone=' ';
    /**
     * UTM projection longitude index.
     */
    public int uTMLonZone=-1;
    /**
     * Creates a new empty instance of GeoPoint.
     * The new point is initialised.
     */
    public GeoPoint() {
        initialise();
    }
    
    // ACCESSORS
    
    public double getLat() {
        if(lat>=dummyAng) computeLatLonAlt();
        return lat;
    }
    
    public double getLon() {
        if(lon>=dummyAng) computeLatLonAlt();
        return lon;
    }
    
    // CLASS METHODS
    
    /**
     * Put default (unusable values in this object so that it will be ready for reuse.
     */
    public void initialise() {
        lat=dummyAng;
        lon=dummyAng;
        altSeaLevel=-1;
        myX=dummyCoord;
        myY=dummyCoord;
        myZ=dummyCoord;
        uTMEasting=0;
        uTMNorthing=0;
        uTMLatZone=' ';
        uTMLonZone=-1;
    }
    
    /**
     * Copy all elements of this point in a new instance.
     * The measured time is also copied in a new Time object.
     * @return New GeoPoint with identical data
     * @see ewe.util.Copyable#getCopy
     */
    public Object getCopy() {
        GeoPoint pt=new GeoPoint();
        pt.copyFrom(this);
        return pt;
    }
    
    /**
     * Copy all elements of point param into <b>this</b> instance.
     * @param param Point to copy from.
     */
    public void copyFrom(Object param) {
        if(!(param instanceof GeoPoint)) {
            return;
        }
        GeoPoint pt= (GeoPoint) param;
        altSeaLevel=pt.altSeaLevel;
        lat=pt.lat;
        lon=pt.lon;
        myX=pt.myX;
        myY=pt.myY;
        myZ=pt.myZ;
        uTMEasting=pt.uTMEasting;
        uTMLatZone=pt.uTMLatZone;
        uTMLonZone=pt.uTMLonZone;
        uTMNorthing=pt.uTMNorthing;
    }
    
    /**
     * Calculate the surface distance in metres from this GeoPoint to the
     * given GeoPoint.
     * @param pointTo Point towards the distance is computed.
     * @return the surface distance in m
     * @since 1.0
     */
    public double distance(GeoPoint pointTo) {
        if(pointTo.myX>=GeoPoint.dummyCoord) {
            pointTo.computeCartesian();
        }
        if(this.myX>=GeoPoint.dummyCoord) {
            computeCartesian();
        }
        
        double d;
        double delta;
        delta=pointTo.myX-myX;
        d = delta*delta;
        delta=pointTo.myY-myY;
        d += delta*delta;
        delta=pointTo.myZ-myZ;
        d += delta*delta;
        if(d<0) {
            d=-d;
        }
        d=Math.sqrt(d);
        return d;
    }
    
    /**
     * Convert UTM reference (if initialised) to latitude and longitude.
     *
     */
    public void computeUTMtoLatLng() {
        double UTM_F0 = 0.9996;
        double a = RefEll.WGS84.getMaj();
        double eSquared = RefEll.WGS84.getEcc();
        double ePrimeSquared = eSquared / (1.0 - eSquared);
        double e1 = (1 - Math.sqrt(1 - eSquared)) / (1 + Math.sqrt(1 - eSquared));
        double x = uTMEasting - 500000.0;
        double y = uTMNorthing;
        int zoneNumber = uTMLonZone;
        char zoneLetter = uTMLatZone;
        
        double longitudeOrigin = (zoneNumber - 1.0) * 6.0 - 180.0 + 3.0;
        
        // Correct y for southern hemisphere
        if ((zoneLetter - 'N') < 0) {
            y -= 10000000.0;
        }
        
        double m = y / UTM_F0;
        double mu =
                m
                / (a * (1.0 - eSquared / 4.0 - 3.0 * eSquared * eSquared / 64.0 - 5.0 * Math
                .pow(eSquared, 3.0) / 256.0));
        
        double phi1Rad =
                mu + (3.0 * e1 / 2.0 - 27.0 * Math.pow(e1, 3.0) / 32.0)
                * Math.sin(2.0 * mu)
                + (21.0 * e1 * e1 / 16.0 - 55.0 * Math.pow(e1, 4.0) / 32.0)
                * Math.sin(4.0 * mu) + (151.0 * Math.pow(e1, 3.0) / 96.0)
                * Math.sin(6.0 * mu);
        
        double n =
                a / Math.sqrt(1.0 - eSquared * Math.sin(phi1Rad) * Math.sin(phi1Rad));
        double t = Math.tan(phi1Rad) * Math.tan(phi1Rad);
        double c = ePrimeSquared * Math.cos(phi1Rad) * Math.cos(phi1Rad);
        double r =
                a
                * (1.0 - eSquared)
                / Math.pow(1.0 - eSquared * Math.sin(phi1Rad) * Math.sin(phi1Rad),
                1.5);
        double d = x / (n * UTM_F0);
        
        double latitude =
                (phi1Rad - (n * Math.tan(phi1Rad) / r)
                * (d
                * d
                / 2.0
                - (5.0 + (3.0 * t) + (10.0 * c) - (4.0 * c * c) - (9.0 * ePrimeSquared))
                * Math.pow(d, 4.0) / 24.0 + (61.0 + (90.0 * t) + (298.0 * c)
                + (45.0 * t * t) - (252.0 * ePrimeSquared) - (3.0 * c * c))
                * Math.pow(d, 6.0) / 720.0))
                * (180.0 / Math.PI);
        
        double longitude =
                longitudeOrigin
                + ((d - (1.0 + 2.0 * t + c) * Math.pow(d, 3.0) / 6.0 + (5.0
                - (2.0 * c) + (28.0 * t) - (3.0 * c * c)
                + (8.0 * ePrimeSquared) + (24.0 * t * t))
                * Math.pow(d, 5.0) / 120.0) / Math.cos(phi1Rad))
                * (180.0 / Math.PI);
        
        this.lat = latitude;
        this.lon = longitude;
        
        return;
    }
    
    /**
     * Work out the UTM latitude zone from the latitude.
     *
     * @param latitude
     *          the latitude to find the UTM latitude zone for
     * @return the UTM latitude zone for the given latitude
     * @since 1.0
     */
    public static char getUTMLatitudeZoneLetter(double latitude) {
        if ((84 >= latitude) && (latitude >= 72))
            return 'X';
        else if ((72 > latitude) && (latitude >= 64))
            return 'W';
        else if ((64 > latitude) && (latitude >= 56))
            return 'V';
        else if ((56 > latitude) && (latitude >= 48))
            return 'U';
        else if ((48 > latitude) && (latitude >= 40))
            return 'T';
        else if ((40 > latitude) && (latitude >= 32))
            return 'S';
        else if ((32 > latitude) && (latitude >= 24))
            return 'R';
        else if ((24 > latitude) && (latitude >= 16))
            return 'Q';
        else if ((16 > latitude) && (latitude >= 8))
            return 'P';
        else if ((8 > latitude) && (latitude >= 0))
            return 'N';
        else if ((0 > latitude) && (latitude >= -8))
            return 'M';
        else if ((-8 > latitude) && (latitude >= -16))
            return 'L';
        else if ((-16 > latitude) && (latitude >= -24))
            return 'K';
        else if ((-24 > latitude) && (latitude >= -32))
            return 'J';
        else if ((-32 > latitude) && (latitude >= -40))
            return 'H';
        else if ((-40 > latitude) && (latitude >= -48))
            return 'G';
        else if ((-48 > latitude) && (latitude >= -56))
            return 'F';
        else if ((-56 > latitude) && (latitude >= -64))
            return 'E';
        else if ((-64 > latitude) && (latitude >= -72))
            return 'D';
        else if ((-72 > latitude) && (latitude >= -80))
            return 'C';
        else
            return 'Z';
    }
    /**
     * Convert this GeoPoint latitude and longitude values to a UTM reference.
     * @since 1.0
     */
    public void computeLatLonToUTMRef() {
        double UTM_F0 = 0.9996;
        double a = RefEll.WGS84.getMaj();
        double eSquared = RefEll.WGS84.getEcc();
        double longitude = this.lon;
        double latitude = this.lat;
        
        double latitudeRad = latitude * (Math.PI / 180.0);
        double longitudeRad = longitude * (Math.PI / 180.0);
        int longitudeZone = (int) Math.floor((longitude + 180.0) / 6.0) + 1;
        
        // Special zone for Norway
        if (latitude >= 56.0 && latitude < 64.0 && longitude >= 3.0
                && longitude < 12.0) {
            longitudeZone = 32;
        }
        
        // Special zones for Svalbard
        if (latitude >= 72.0 && latitude < 84.0) {
            if (longitude >= 0.0 && longitude < 9.0) {
                longitudeZone = 31;
            } else if (longitude >= 9.0 && longitude < 21.0) {
                longitudeZone = 33;
            } else if (longitude >= 21.0 && longitude < 33.0) {
                longitudeZone = 35;
            } else if (longitude >= 33.0 && longitude < 42.0) {
                longitudeZone = 37;
            }
        }
        
        this.uTMLonZone=longitudeZone;
        
        double longitudeOrigin = (longitudeZone - 1) * 6 - 180 + 3;
        double longitudeOriginRad = longitudeOrigin * (Math.PI / 180.0);
        
        this.uTMLatZone = getUTMLatitudeZoneLetter(latitude);
        
        double ePrimeSquared = (eSquared) / (1 - eSquared);
        
        double n =
                a
                / Math.sqrt(1 - eSquared * Math.sin(latitudeRad)
                * Math.sin(latitudeRad));
        double t = Math.tan(latitudeRad) * Math.tan(latitudeRad);
        double c = ePrimeSquared * Math.cos(latitudeRad) * Math.cos(latitudeRad);
        double A = Math.cos(latitudeRad) * (longitudeRad - longitudeOriginRad);
        
        double M =
                a
                * ((1 - eSquared / 4 - 3 * eSquared * eSquared / 64 - 5 * eSquared
                * eSquared * eSquared / 256)
                * latitudeRad
                - (3 * eSquared / 8 + 3 * eSquared * eSquared / 32 + 45
                * eSquared * eSquared * eSquared / 1024)
                * Math.sin(2 * latitudeRad)
                + (15 * eSquared * eSquared / 256 + 45 * eSquared * eSquared
                * eSquared / 1024) * Math.sin(4 * latitudeRad) - (35
                * eSquared * eSquared * eSquared / 3072)
                * Math.sin(6 * latitudeRad));
        
        this.uTMEasting =
                (UTM_F0
                * n
                * (A + (1 - t + c) * Math.pow(A, 3.0) / 6 + (5 - 18 * t + t * t
                + 72 * c - 58 * ePrimeSquared)
                * Math.pow(A, 5.0) / 120) + 500000.0);
        
        this.uTMNorthing =
                (UTM_F0 * (M + n
                * Math.tan(latitudeRad)
                * (A * A / 2 + (5 - t + (9 * c) + (4 * c * c)) * Math.pow(A, 4.0)
                / 24 + (61 - (58 * t) + (t * t) + (600 * c) - (330 * ePrimeSquared))
                * Math.pow(A, 6.0) / 720)));
        
        // Adjust for the southern hemisphere
        if (latitude < 0) {
            this.uTMNorthing += 10000000.0;
        }
        
        return;
    }
    
    /**
     * Compute lat,long, alt coordinates of this point from its X,Y,Z.
     * @since 1.0
     */
    public void computeLatLonAlt() {
        lat=180.0d/Math.PI*Math.asin(myZ/ER);
        if((myX==0.0d)&&(myY==0.0d)) {
            lon=0.0d; // undefined
        } else {
            lon=180.0d/Math.PI*Math.asin(myY/Math.sqrt(myX*myX+myY*myY));
        }
        if(myX<0) {
            if(myY>0) lon = 180.0d-lon;
            else lon = -180.0d-lon;
        }
        altSeaLevel=0.0d;
    }
    
    /**
     * Compute X,Y,Z coordinates of this point from long and lat.
     * @since 1.0
     */
    public void computeCartesian() {
        if(this.lat>=1E3) { // this point is not valid
            return;
        }
        
        double latRad= Math.PI*this.lat/180.0d;
        double lngRad = Math.PI*this.lon/180.0d;
        
        //CourseLog.logger.addToLog("\n GeoPoint#distance- lngTo : "+lngTo);
        myZ = ER*Math.sin(latRad);
        double proj=ER*Math.cos(latRad);
        myX=proj*Math.cos(lngRad);
        myY=proj*Math.sin(lngRad);
    }
    
    /**
     * Compute heading to another point <I>pt</I> in degrees.
     * Value is between 0° and 360°.
     * @param pt Point for which the direction is computed.
     * @return Heading in degrees.
     */
    public double headingTo(GeoPoint pt) {
        // generate cartesian data for the points
        if(myX>=dummyCoord) {
            computeCartesian();
        }
        if( pt.myX>=dummyCoord) {
            pt.computeCartesian();
        }
        // Local referential (North, East)
        double cosLat= Math.cos(lat*Math.PI/180.0d);
        double sinLat= Math.sin(lat*Math.PI/180.0d);
        double cosLon= Math.cos(lon*Math.PI/180.0d);
        double sinLon= Math.sin(lon*Math.PI/180.0d);
        
        // North vector
        double vNx=-cosLon*sinLat;
        double vNy=-sinLon*sinLat;
        double vNz=cosLat;
        
        // East vector
        double vEx=-sinLon;
        double vEy=cosLon;
        double vEz=0.0d;
        
        // Direction vector
        double vDx=pt.myX-myX;
        double vDy=pt.myY-myY;
        double vDz=pt.myZ-myZ;
        double invDist = Math.sqrt(vDx*vDx+vDy*vDy+vDz*vDz);
        if(invDist==0.0d) return 0.0d; // No distance means no direction
        
        vDx = vDx/invDist;
        vDy = vDy/invDist;
        vDz = vDz/invDist;
        
        // Scalar product with North gives cos(heading)
        double hdg=180.0d*Math.acos(vNx*vDx+vNy*vDy+vNz*vDz)/Math.PI;
        // get sign of projection on East
        double sinH=vDx*vEx+vDy*vEy; // vEz=0
        if(sinH<0) {
            hdg = 360-hdg;
        }
        return hdg;
    }
    
    /**
     * Compute point by translating a first one along 2 vectors.
     * This method is re-entrant : it can be applied to a point which is
     * also the result of the operation: x=x.moveBy(2,3,x) is valid.
     */
    public GeoPoint moveBy(boolean setAlt0, GeoDirection d1, double c1,
            GeoDirection d2, double c2, GeoPoint dest) {
        if(dest==null) dest=new GeoPoint();
        if(myX>=GeoPoint.dummyCoord) {
            computeCartesian();
        }
        
        dest.myX=myX+d1.x*c1+d2.x*c2;
        dest.myY=myY+d1.y*c1+d2.y*c2;
        dest.myZ=myZ+d1.z*c1+d2.z*c2;
        
        if(setAlt0) {
            double ratio=GeoPoint.ER/Math.sqrt(dest.myX*dest.myX+
                    dest.myY*dest.myY+
                    dest.myZ*dest.myZ);
            dest.myX *=ratio;
            dest.myY *=ratio;
            dest.myZ *=ratio;
        }
        
        dest.computeLatLonAlt();
        dest.altSeaLevel=0;
        
        return dest;
    }
    
    /**
     * Compute point coordinates in a given vector.
     * Puts results in table dest which must have 2 places.
     */
    public void coordinatesIn(GeoPoint orig, GeoDirection dNorth,
            GeoDirection dEast, double[] dest) {
        if(orig.myX>=GeoPoint.dummyCoord) {
            GeoDirection.vectorsNorthEast(orig,dNorth,dEast);
        }
        if(myX>=dummyCoord) {
            computeCartesian();
        }
        
        // Compute direction vector from origin to this point
        double dX= this.myX-orig.myX;
        double dY= this.myY-orig.myY;
        double dZ= this.myZ-orig.myZ;
        
        dest[0]=dX*dEast.x+dY*dEast.y+dZ*dEast.z;
        dest[1]=dX*dNorth.x+dY*dNorth.y+dZ*dNorth.z;
    }
        
    /**
     * Compute direction vector between 2 points.
     * @param pTo point TO.
     * @param dest GeoDirection reference to put result in. If <I><B>null</B></I>
     * a new object is allocated.
     * @return Computed direction. Should be <I>dest</I> or a new GeoPoint
     * object.
     */
    public GeoDirection directionTo(GeoPoint pTo, GeoDirection dest) {
        if(dest==null) {
            dest=new GeoDirection();
        }
        if(myX>=dummyCoord) {
            computeCartesian();
        }
        if(pTo.myX>=dummyCoord) {
            pTo.computeCartesian();
        }
        
        dest.x=pTo.myX-this.myX;
        dest.y=pTo.myY-this.myY;
        dest.z=pTo.myZ-this.myZ;
        
        return dest;
    }
    
    /**
     * Convert this point UTM reference to a String representation for printing out.
     *
     * @return a String representation of this GeoPoint
     * @since 1.0
     */
    public String toUTMString() {
        return uTMLonZone + Character.toString(uTMLatZone) + " " + uTMEasting + " "
                + uTMNorthing;
    }
    
    /**
     * Get a Lat/Lon String representation of this GeoPoint object.
     *
     * @return a Lat/Lon String representation of this GeoPoint object.
     * @since 1.0
     */
    public String toLatLonString() {
        return "(" + this.lat + ", " + this.lon + ")";
    }
    
    /**
     * Converts the latitude of <B>this</B> point into a String.
     * @return Formatted latitude.
     * @see Converters#ddmmss(double,int)
     */
    public String ddmmssLat() {
        return Converters.ddmmss(lat,Converters.DDMMSS_LAT);
    }
    
    /**
     * Converts the longitude of <B>this</B> point into a String.
     * @return Fromatted longitude.
     * @see Converters#ddmmss(double,int)
     */
    public String ddmmssLon() {
        return Converters.ddmmss(lon,Converters.DDMMSS_LON);
    }
    
    /**
     * Formats the altitude of this point.
     * @return Formatted altitude.
     * @see Converters#formatAltitude(double)
     */
    public String formatAlt() {
        return Converters.formatAltitude(altSeaLevel);
    }
    
    // OVERRIDDEN METHODS
    
    /**
     * Display lat,lon as a string.
     */
    public String toString() {
        return "("+lat+","+lon+")";
    }
    
    /**
     * Formats this track point in a buffer so that it can be
     * written to the log.
     * Values are formatted in order to be readable as csv values.
     * @param buff buffer to write in.
     * @return Length of the formatted values in the buffer.
     */
    public int formatInBuffer(byte[] buff) {
        if(lat>=dummyAng) computeLatLonAlt();
        computeLatLonToUTMRef();
        
        int cur=0;
        // Write latitude
        cur=Converters.writeDblInBuffer(buff, cur, 9, 6, this.lat);
        buff[cur]=','; cur++;
        // Write longitude
        cur=Converters.writeDblInBuffer(buff, cur, 10, 6, this.lon);
        buff[cur]=','; cur++;
        // Write altitude
        cur=Converters.writeDblInBuffer(buff, cur, 7, 1, this.altSeaLevel);
        buff[cur]=','; cur++;
        // Write UTM zone Id (lon+lat)
        cur=Converters.writeIntInBuffer(buff, cur, 2, this.uTMLonZone);
        buff[cur]=(byte) this.uTMLatZone; cur++;
        buff[cur]=','; cur++;
        // Write UTM easting
        cur=Converters.writeDblInBuffer(buff, cur, 9, 2, this.uTMEasting);
        buff[cur]=','; cur++;
        // Write UTM northing
        cur=Converters.writeDblInBuffer(buff, cur, 10, 2, this.uTMNorthing);

        buff[cur]='\n'; cur++;
        return cur;
    }

    /**
     * Fill in attributes of this point from a string generated by the
     * <B>PositionTracer</B> log function.
     * @param logLine Input string to interpret.
     * @return true if the string looks correct, false otherwise.
     */
    public boolean readFromLogLine(String logLine) {
        double[] vDbl=new double[5];
        int[] vInt=new int[0];
        char[] vCh=new char[3];
        
        // Parse content
        String sequenceLogGeo="VVVCCCVV";
        Converters.parseCSVSentence(sequenceLogGeo, logLine.getBytes(),0,
                logLine.length()-1, vDbl, vInt, vCh);
        // Put values in fields
        this.lat=vDbl[0]; // Signed latitude
        this.lon=vDbl[1]; // Signed longitude
        if((lat>90.0d)||(lat<-90.0d)||(lon>180.0d)||(lon<-180.0d)) {
            return false;
        }
        this.altSeaLevel=vDbl[2]; // Altitude
        this.computeLatLonToUTMRef();
        this.computeCartesian();
        
        return true;
    }
}